
// Author: Rafal Pielech 
// E-mail: rafalpielech@hotmail.com
// Date:   2003.08.31

#ifndef MATRIX_H
#define MATRIX_H

#include <math.h>
#include "svector.h"


template < class ElType >
class Matrix
{
      private:
                 ElType  Elements[4][4];
                 ElType  Base[16];
                 
                 void   SetElement( const short col , const short row , const ElType Value );
                 ElType GetElement( const short col , const short row ) const;
               
      public:

// --- constructors ---------------------------------------------------------------------

              Matrix( void );
               
// --- constructors ---------------------------------------------------------------------

// --- operators ------------------------------------------------------------------------                         
              
                   friend short operator == ( const Matrix<ElType> left , const Matrix<ElType> right )
                          {
                                short i,j;
      
                                for( i = 0; i < 4; i++ )
                                {
                                      for( j = 0; j < 4; j++ )
                                      {
                                             if( left.Elements[i][j] != right.Elements[i][j] ) return 0;
                                      }
                                }

                               return 1;
                          }      
 
                 Matrix<ElType> operator  = ( const Matrix<ElType> right );
          friend Matrix<ElType> operator  * ( const Matrix<ElType> left , const Matrix<ElType> right )
                 {
                        short i , j , k;
                        ElType Value = 0.0;
      
                        Matrix<ElType> tmp;
      
                        for( i = 0; i < 4; i++ )
                        {
                             for( j = 0; j < 4; j++ )
                             {
                                   for( k = 0; k < 4; k++ )
                                   {
                                          Value += left.Elements[k][i] * right.Elements[j][k];
                                   }
                  
                                tmp.Elements[j][i] = Value;
                                Value = 0.0;
                              }
                        }

                      return tmp;   
                 }

       
// --- operators ------------------------------------------------------------------------

// --- other member functions -----------------------------------------------------------

                     const ElType * GetArray( void );
                   
                           void LoadIdentity( void );
                           
                           void Translate( const ElType Tx , const ElType Ty , const ElType Tz );
                           void Translate( const Simple_Vector<ElType> Trans );
                           
                           void Rotate( ElType Angle , const char Axis );
                           
          Simple_Vector<ElType> MultVector( const Simple_Vector<ElType> Vec ) const;

                           void Print( void );

// --- other member functions -----------------------------------------------------------

// --- destructor ---------------------------------------------------------------------
               
// --- destructor ---------------------------------------------------------------------
};


//---------------------------------------------------------------------------------------

template< class ElType >
void 
Matrix<ElType> :: SetElement( const short col , const short row , const ElType Value )
{
      if( ((row < 1)||(row > 4)) || ((col < 1)||(col > 4)) )
      {
            return;
      }

      Elements[col-1][row-1] = Value;
}

//---------------------------------------------------------------------------------------

template< class ElType >
ElType
Matrix<ElType> :: GetElement( const short col , const short row ) const
{
      if( ((row < 0)||(row > 3)) || ((col < 0)||(col > 3)) )
      {
            return 0.0;
      }

      return Elements[col][row];
}


//---------------------------------------------------------------------------------------

template< class ElType >
const ElType *
Matrix<ElType> :: GetArray( void )
{
      short i,j,k = 0;
      
      for( i = 0; i < 4; i++ )
      {
            for( j = 0; j < 4; j++ )
            {
                  Base[ k++ ] = Elements[i][j];
            }
      }
      
      return Base;
}

//---------------------------------------------------------------------------------------

template< class ElType >
void
Matrix<ElType> :: LoadIdentity( void )
{
      short i,j;
      
      for( i = 0; i < 4; i++ )
      {
            for( j = 0; j < 4; j++ )
            {
                  if( i == j )
                  {
                        Elements[i][j] = 1.0;
                  }
                  else
                  {
                        Elements[i][j] = 0.0;
                  }
            }
      }
}       
      
//---------------------------------------------------------------------------------------
template< class ElType >
Matrix<ElType> :: Matrix( void )
{
      LoadIdentity();
}

//---------------------------------------------------------------------------------------

template< class ElType >
void 
Matrix<ElType> :: Translate( const ElType Tx , const ElType Ty , const ElType Tz )
{
      Matrix<ElType> tmp;
      
      tmp.SetElement( 4 , 1 , Tx );
      tmp.SetElement( 4 , 2 , Ty );
      tmp.SetElement( 4 , 3 , Tz );
      
      (*this) = ((*this) * tmp);
}

//---------------------------------------------------------------------------------------

template< class ElType >
void 
Matrix<ElType> :: Translate( const Simple_Vector<ElType> Trans )
{
      Matrix<ElType> tmp;
      
      tmp.SetElement( 4 , 1 , Trans.GetElement(0) );
      tmp.SetElement( 4 , 2 , Trans.GetElement(1) );
      tmp.SetElement( 4 , 3 , Trans.GetElement(2) );
      
      (*this) = ((*this) * tmp);
}

//---------------------------------------------------------------------------------------


template< class ElType >
void 
Matrix<ElType> :: Rotate( ElType Angle , const char Axis )
{
      ElType radian_deg = 180/M_PI;
      Angle /= radian_deg;

      Matrix<ElType> tmp;
      
      ElType Sinus   = static_cast< ElType >( sin( (double)Angle ) );
      ElType Cosinus = static_cast< ElType >( cos( (double)Angle ) );
      
      switch( Axis )
      {
            case 'x':
            case 'X':
                        tmp.SetElement(2,2,Cosinus);
                        tmp.SetElement(3,3,Cosinus);
                        
                        tmp.SetElement(2,3,Sinus);
                        tmp.SetElement(3,2,-Sinus);
                        break;

            case 'y':
            case 'Y':
                        tmp.SetElement(1,1,Cosinus);
                        tmp.SetElement(3,3,Cosinus);
                        
                        tmp.SetElement(3,1,Sinus);
                        tmp.SetElement(1,3,-Sinus);
                        break;

            case 'z':
            case 'Z':
                        tmp.SetElement(1,1,Cosinus);
                        tmp.SetElement(2,2,Cosinus);
                        
                        tmp.SetElement(1,2,Sinus);
                        tmp.SetElement(2,1,-Sinus);
                        break;
      }
      
      
      (*this) = (*this) * tmp;
}

//---------------------------------------------------------------------------------------

template < class ElType >
Simple_Vector<ElType>
Matrix<ElType> :: MultVector( const Simple_Vector<ElType> Vec ) const
{
      Simple_Vector<ElType> Result;
      
      ElType v[4];
      
      v[0] = Vec.GetElement(0);
      v[1] = Vec.GetElement(1);
      v[2] = Vec.GetElement(2);
      v[3] = Vec.GetElement(3);
      
      
      Result.SetElement(0 , Elements[0][0] * v[0] + Elements[1][0] * v[1] + Elements[2][0] * v[2] + Elements[3][0] * v[3] );
      Result.SetElement(1 , Elements[0][1] * v[0] + Elements[1][1] * v[1] + Elements[2][1] * v[2] + Elements[3][1] * v[3] );
      Result.SetElement(2 , Elements[0][2] * v[0] + Elements[1][2] * v[1] + Elements[2][2] * v[2] + Elements[3][2] * v[3] );
      Result.SetElement(3 , Elements[0][3] * v[0] + Elements[1][3] * v[1] + Elements[2][3] * v[2] + Elements[3][3] * v[3] );
      
    return Result;  
}

//---------------------------------------------------------------------------------------

template < class ElType >
void
Matrix<ElType> :: Print( void )
{
      cout << "\n" << endl;
      
      cout << Elements[0][0] << " " << Elements[1][0] << " " << Elements[2][0] << " " << Elements[3][0] << endl;
      cout << Elements[0][1] << " " << Elements[1][1] << " " << Elements[2][1] << " " << Elements[3][1] << endl;
      cout << Elements[0][2] << " " << Elements[1][2] << " " << Elements[2][2] << " " << Elements[3][2] << endl;
      cout << Elements[0][3] << " " << Elements[1][3] << " " << Elements[2][3] << " " << Elements[3][3] << endl;
      
      cout << "\n" << endl;
}

//---------------------------------------------------------------------------------------

template < class ElType >
Matrix<ElType>
Matrix<ElType> :: operator = ( const Matrix<ElType> right )
{
      short i,j;
      
      for( i = 0; i < 4; i++ )
      {
            for( j = 0; j < 4; j++ )
            {
                  Elements[i][j] = right.GetElement(i,j);
            }
      }
     
   return (*this);   
}

//---------------------------------------------------------------------------------------
#endif MATRIX_H